#include "includes.h"
#include "servo.h"

uint8 servo_time;

extern double uni[4];

double servo_error[3][2];
double servo_duty;

void servo_error_calc(void){
    if(++servo_time==3)
        servo_time=0;

    servo_error[servo_time][0]=1/uni[0]-1/uni[1];
    servo_error[servo_time][1]=1/uni[2]-1/uni[3];
}

void pd_control(void){
    const double kp=23,kd=550;
    servo_duty=kp*servo_error[servo_time][0]+kd*(servo_error[servo_time][0]-servo_error[IN_RANGE(servo_time-1, 3)][0]);
    servo_duty=MAX(servo_duty, SERVO_DUTY_MIN-SERVO_DUTY_MIDDLE);
    servo_duty=MIN(servo_duty, SERVO_DUTY_MIDDLE-SERVO_DUTY_MAX);
}

void servo_control(void){
    servo_error_calc();

    pd_control();
}
